#ifndef WHEEL_FACTOR_H_
#define WHEEL_FACTOR_H_
#include <iostream>
#include <vector>
#include <memory>

#include <ceres/ceres.h>

#include "wheel_preintegration.h"
#include "state.h"

class WheelFactor  : public ceres::SizedCostFunction<3, 8, 5>{
public:  
   WheelFactor(std::shared_ptr<WheelPreIntegration> wheel_preintegration);

   Eigen::Matrix<double, 3, 3> GetCov() {
      return wheel_preintegration_->cov_;
   }

   virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
      double theta_i = parameters[0][0];
      Eigen::Vector2d vi(parameters[0][1], parameters[0][2]);
      Eigen::Vector2d pi(parameters[0][3], parameters[0][4]);
      double bg = parameters[0][5];
      Eigen::Vector2d ba(parameters[0][6], parameters[0][7]);
      double theta_j = parameters[1][0];
      Eigen::Vector2d vj(parameters[1][1], parameters[1][2]);
      Eigen::Vector2d pj(parameters[1][3], parameters[1][4]);

      Eigen::Matrix2d Ri = SO2(theta_i).matrix();
      Eigen::Vector2d ri = pi;
      double ai = theta_i;
      double aj = theta_j;
      Eigen::Vector2d rj = pj;

      Eigen::Map<Eigen::Matrix<double, 3, 1>> residual(residuals);
      residual.setZero();

      residual[0] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log();
      residual.tail<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;

      Eigen::Matrix<double, 3, 3> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 3, 3>>(wheel_preintegration_->cov_.inverse()).matrixL().transpose();
      residual = sqrt_info * residual;

      cout << " wheel residual ==== : " << residual.transpose() << std::endl;
    
      // compute jacobian matrix
      Eigen::Vector2d rij = rj-ri;
      Eigen::Vector2d rij_x(-rij[1], rij[0]);
      // 公式（25）, （十）--- 2.2
      // ti, vxi, vyi, pxi, pyi, bgi, baxi, bayi, tj, vxj, vyj, pxj, pyj; right
      if (jacobians) {
         if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 3, 8, Eigen::RowMajor>> jacobianXi(jacobians[0]);
            jacobianXi.setZero();
            jacobianXi(0,0) = -1;  // et/ti
            jacobianXi.block<2,1>(1,0) = -Ri.transpose() * rij_x; // ep/ti 
            jacobianXi.block<2,2>(1,3) = -Ri.transpose(); // ep/p  
         }
         if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 3, 5, Eigen::RowMajor>> jacobianXj(jacobians[0]);
            jacobianXj.setZero();
            jacobianXj(0,0) = 1;
            jacobianXj.block<2,2>(1,3) = Ri.transpose(); 
         }
      }
   }

private:
   std::shared_ptr<WheelPreIntegration> wheel_preintegration_;
};

#endif 
